#include "CollisionTest.h"

bool CollisionTest::IsColliding(D3DXVECTOR3 Point1, D3DXVECTOR3 Point2)
{
	//This is essentially a pointless (get it? POINTless! haha....)
	//function 2 points being on the exact same coordinate is practically impossible
	if(Point1 == Point2)
		return true;
	else
		return false;
}

bool CollisionTest::IsColliding(CollisionSphere Sphere, CollisionBox Box)
{

	//Not a perfect or "correct" algorithm, but it works enough for now
	D3DXVECTOR3 TestPoint;

	TestPoint = Sphere.Position - Box.Position;
	float d = D3DXVec3LengthSq(&TestPoint);
	if( d <= Sphere.Radius*Sphere.Radius )
		return true;
	else 
		return false;
}

bool CollisionTest::IsColliding(CollisionBox Box, D3DXVECTOR3 Point)
{
	//Here is where the ugliest code in the world is going to go :)
	if(Point.x < (Box.Points[0].x+Box.Position.x)) if(Point.y < (Box.Points[0].y+Box.Position.y)) if(Point.z < (Box.Points[0].z+Box.Position.z))
		if(Point.x > (Box.Points[1].x+Box.Position.x)) if(Point.y > (Box.Points[1].y+Box.Position.y)) if(Point.z < (Box.Points[1].z+Box.Position.z))
			if(Point.x < (Box.Points[2].x+Box.Position.x)) if(Point.y > (Box.Points[2].y+Box.Position.y)) if(Point.z < (Box.Points[2].z+Box.Position.z))
				if(Point.x > (Box.Points[3].x+Box.Position.x)) if(Point.y < (Box.Points[3].y+Box.Position.y)) if(Point.z < (Box.Points[3].z+Box.Position.z))

					if(Point.x < (Box.Points[4].x+Box.Position.x)) if(Point.y < (Box.Points[4].y+Box.Position.y)) if(Point.z > Box.Points[4].z+Box.Position.z)
						if(Point.x < (Box.Points[5].x+Box.Position.x)) if(Point.y > (Box.Points[5].y+Box.Position.y)) if(Point.z > (Box.Points[5].z+Box.Position.z))
							if(Point.x > (Box.Points[6].x+Box.Position.x)) if(Point.y < (Box.Points[6].y+Box.Position.y)) if(Point.z > (Box.Points[6].z+Box.Position.z))
								if(Point.x > (Box.Points[7].x+Box.Position.x)) if(Point.y > (Box.Points[7].y+Box.Position.y)) if(Point.z > (Box.Points[7].z+Box.Position.z))
									return true;//*/

	return false;
	//OLD CODE
	/*if(Point.x < (Box.Position.x + Box.HalfVector.x))
	if(Point.y < (Box.Position.y + Box.HalfVector.y))
	if(Point.z < (Box.Position.z + Box.HalfVector.z))
	if(Point.x > (Box.Position.x - Box.HalfVector.x))
	if(Point.y > (Box.Position.y - Box.HalfVector.y))
	if(Point.z > (Box.Position.z - Box.HalfVector.z))
	return true;//*/
}

bool CollisionTest::IsColliding(PhysicsBox *Box1, PhysicsBox *Box2)
{
	//Final Return variable, probably could cut this out, but it makes it much more readible to me.
	bool RetVal = false; 

	//Temp variables, for an optimization, move the matrix to the CollisionTest class, or use quaternions
	D3DXVECTOR3 TestPoint;
	D3DXMATRIX temp;

	//Sets the matrix to a rotation of the opposite of what the box1's is
	D3DXMatrixRotationYawPitchRoll(&temp, -Box1->Angles.x, -Box1->Angles.y, -Box1->Angles.z);

	//Checks every point of box2 against box1
	for(UINT i = 0; i < Box2->BoundingBox.Points.size()-8; ++i)
	{

		//Rotates the point by the opposite of Box1, essentially rotating it into the object's local space
		D3DXVec3TransformCoord(&TestPoint, &(Box2->BoundingBox.Points[i] + (Box2->GetPosition() - Box1->GetPosition())), &temp);

		//Checks collision
		if(IsColliding(Box1, TestPoint + Box1->GetPosition()))
			RetVal = true;
		if(RetVal)
			return RetVal;
	}

	//Sets the matrix to a rotation of the opposite of what the box1's is
	D3DXMatrixRotationYawPitchRoll(&temp, -Box2->Angles.x,-Box2->Angles.y, -Box2->Angles.z);

	//Checks every point of box1 against box2
	for(UINT i = 0; i < Box1->BoundingBox.Points.size()-8; ++i)
	{
		//Rotates the point by the opposite of Box2, essentially rotating it into the object's local space
		D3DXVec3TransformCoord(&TestPoint, &(Box1->BoundingBox.Points[i] + (Box2->GetPosition() - Box1->GetPosition())), &temp);
		
		//Checks collision
		if(IsColliding(Box2, TestPoint + Box2->GetPosition()))
			RetVal = true;
		if(RetVal)
			return RetVal;
	}

	return RetVal;
}

bool CollisionTest::IsColliding(PhysicsBox *Box, D3DXVECTOR3 Point)
{
	//Temp variable for collision testing
	CollisionBox TempBox;

	//Sets the temporary variable's points to the AABB of the original box
	//This works because the point is passed in as a local space point
	for(UINT i = 0; i < Box->BoundingBox.Points.size()-8; ++i)
	{
		TempBox.Points[i] = Box->BoundingBox.Points[i+8];
	}

	TempBox.Position = Box->GetPosition();
	TempBox.HalfVector = Box->BoundingBox.HalfVector;

	//The same code for checking the collision box.
	if(Point.x < (TempBox.Points[0].x+TempBox.Position.x)) if(Point.y < (TempBox.Points[0].y+TempBox.Position.y)) if(Point.z < (TempBox.Points[0].z+TempBox.Position.z))
		if(Point.x > (TempBox.Points[1].x+TempBox.Position.x)) if(Point.y > (TempBox.Points[1].y+TempBox.Position.y)) if(Point.z < (TempBox.Points[1].z+TempBox.Position.z))
			if(Point.x < (TempBox.Points[2].x+TempBox.Position.x)) if(Point.y > (TempBox.Points[2].y+TempBox.Position.y)) if(Point.z < (TempBox.Points[2].z+TempBox.Position.z))
				if(Point.x > (TempBox.Points[3].x+TempBox.Position.x)) if(Point.y < (TempBox.Points[3].y+TempBox.Position.y)) if(Point.z < (TempBox.Points[3].z+TempBox.Position.z))

					if(Point.x < (TempBox.Points[4].x+TempBox.Position.x)) if(Point.y < (TempBox.Points[4].y+TempBox.Position.y)) if(Point.z > TempBox.Points[4].z+TempBox.Position.z)
						if(Point.x < (TempBox.Points[5].x+TempBox.Position.x)) if(Point.y > (TempBox.Points[5].y+TempBox.Position.y)) if(Point.z > (TempBox.Points[5].z+TempBox.Position.z))
							if(Point.x > (TempBox.Points[6].x+TempBox.Position.x)) if(Point.y < (TempBox.Points[6].y+TempBox.Position.y)) if(Point.z > (TempBox.Points[6].z+TempBox.Position.z))
								if(Point.x > (TempBox.Points[7].x+TempBox.Position.x)) if(Point.y > (TempBox.Points[7].y+TempBox.Position.y)) if(Point.z > (TempBox.Points[7].z+TempBox.Position.z))
									return true;//*/

	return false;
	//OLD CODE
	/*if(Point.x < (Box.Position.x + Box.HalfVector.x))
	if(Point.y < (Box.Position.y + Box.HalfVector.y))
	if(Point.z < (Box.Position.z + Box.HalfVector.z))
	if(Point.x > (Box.Position.x - Box.HalfVector.x))
	if(Point.y > (Box.Position.y - Box.HalfVector.y))
	if(Point.z > (Box.Position.z - Box.HalfVector.z))
	return true;//*/
}


bool CollisionTest::IsColliding(CollisionBox Box1, CollisionBox Box2)
{
	//Tests 1, 1, 1
	if(IsColliding(Box1, Box2.Points[0] + Box2.Position))
		return true;

	//Tests 1, 1, -1
	if(IsColliding(Box1, Box2.Points[1] + Box2.Position))
		return true;

	//Tests 1, -1, -1
	if(IsColliding(Box1, Box2.Points[2] + Box2.Position))
		return true;

	//Tests 1, -1, 1
	if(IsColliding(Box1, Box2.Points[3] + Box2.Position))
		return true;

	//Tests -1, 1, 1
	if(IsColliding(Box1, Box2.Points[4] + Box2.Position))
		return true;

	//Tests -1, -1, 1
	if(IsColliding(Box1, Box2.Points[5] + Box2.Position))
		return true;

	//Tests -1, -1, -1
	if(IsColliding(Box1, Box2.Points[6] + Box2.Position))
		return true;

	//Tests -1, 1, -1
	if(IsColliding(Box1, Box2.Points[7] + Box2.Position))
		return true;

	//---------------------------SECOND BOX TESTING--------------------------------//

	//Tests 1, 1, 1
	if(IsColliding(Box2, Box1.Points[0] + Box1.Position))
		return true;

	//Tests 1, 1, -1
	if(IsColliding(Box2, Box1.Points[1] + Box1.Position))
		return true;

	//Tests 1, -1, -1
	if(IsColliding(Box2, Box1.Points[2] + Box1.Position))
		return true;

	//Tests 1, -1, 1
	if(IsColliding(Box2, Box1.Points[3] + Box1.Position))
		return true;

	//Tests -1, 1, 1
	if(IsColliding(Box2, Box1.Points[4] + Box1.Position))
		return true;

	//Tests -1, -1, 1
	if(IsColliding(Box2, Box1.Points[5] + Box1.Position))
		return true;

	//Tests -1, -1, -1
	if(IsColliding(Box2, Box1.Points[6] + Box1.Position))
		return true;

	//Tests -1, 1, -1
	if(IsColliding(Box2, Box1.Points[7] + Box1.Position))
		return true;

	return false;
}

bool CollisionTest::IsColliding(D3DXVECTOR3 Point1, D3DXVECTOR3 Point2, float Negativedt, D3DXVECTOR3 Velocity1, D3DXVECTOR3 Velocity2)
{
	if(IsColliding((Point1 + (Velocity1*Negativedt)),(Point2 + (Velocity2*Negativedt))))
	{
		return true;
	}

	if(IsColliding((Point1 + (Velocity1*(Negativedt*(1/2)))),(Point2 + (Velocity2*(Negativedt*(1/2))))))
	{
		return true;
	}

	if(IsColliding((Point1 + (Velocity1*(Negativedt*(1/4)))),(Point2 + (Velocity2*(Negativedt*(1/4))))))
	{
		return true;
	}

	if(IsColliding((Point1 + (Velocity1*(Negativedt*(3/4)))),(Point2 + (Velocity2*(Negativedt*(3/4))))))
	{
		return true;
	}
	return false;
}

bool CollisionTest::IsColliding(CollisionSphere Sphere, CollisionBox Box, float Negativedt, D3DXVECTOR3 Velocity1, D3DXVECTOR3 Velocity2)
{
	CollisionSphere TempSphere(Sphere.Position, Sphere.Radius);
	CollisionBox TempBox(Box.Position, Box.HalfVector);
	TempBox.Points = Box.Points;

	TempSphere.Position = Sphere.Position + (Negativedt * Velocity1);
	TempBox.Position = Box.Position + (Negativedt * Velocity1);
	
	if(IsColliding(TempSphere, TempBox))
		return true;

	TempSphere.Position = Sphere.Position + ((Negativedt*1/4) * Velocity1);
	TempBox.Position = Box.Position + ((Negativedt*1/4) * Velocity1);
	
	if(IsColliding(TempSphere, TempBox))
		return true;

	TempSphere.Position = Sphere.Position + ((Negativedt*2/4) * Velocity1);
	TempBox.Position = Box.Position + ((Negativedt*2/4) * Velocity1);
	
	if(IsColliding(TempSphere, TempBox))
		return true;

	TempSphere.Position = Sphere.Position + ((Negativedt*3/4) * Velocity1);
	TempBox.Position = Box.Position + ((Negativedt*3/4) * Velocity1);
	
	if(IsColliding(TempSphere, TempBox))
		return true;

	return false;
}

bool CollisionTest::IsColliding(CollisionBox Box, D3DXVECTOR3 Point, float Negativedt, D3DXVECTOR3 Velocity1, D3DXVECTOR3 Velocity2)
{
	CollisionBox TempBox(Box.Position, Box.HalfVector);
	TempBox.Points = Box.Points;
	
	D3DXVECTOR3 TempPoint = Point;

	TempBox.Position = Box.Position + (Negativedt * Velocity1);
	TempPoint = Point + (Negativedt * Velocity2);

	if(IsColliding(TempBox, TempPoint))
	{
		return true;
	}

	TempBox.Position = Box.Position + ((Negativedt*(1/4)) * Velocity1);
	TempPoint = Point + ((Negativedt*(1/4)) * Velocity2);

	if(IsColliding(TempBox, TempPoint))
	{
		return true;
	}

	TempBox.Position = Box.Position + ((Negativedt*(2/4)) * Velocity1);
	TempPoint = Point + ((Negativedt*(2/4)) * Velocity2);

	if(IsColliding(TempBox, TempPoint))
	{
		return true;
	}

	TempBox.Position = Box.Position + ((Negativedt*(3/4)) * Velocity1);
	TempPoint = Point + ((Negativedt*(3/4)) * Velocity2);

	if(IsColliding(TempBox, TempPoint))
	{
		return true;
	}

	return false;
}

bool CollisionTest::IsColliding(PhysicsBox *Box1, PhysicsBox *Box2, float Negativedt, D3DXVECTOR3 Velocity1, D3DXVECTOR3 Velocity2)
{
	D3DXVECTOR3 OriginalPosition1 = Box1->GetPosition(),
				OriginalPosition2 = Box2->GetPosition();
	bool RetValue = false;

	if(!RetValue)
	{
		Box1->SetPosition(OriginalPosition1 + (Velocity1 * (Negativedt * (2/4))));
		Box2->SetPosition(OriginalPosition2 + (Velocity2 * (Negativedt * (2/4))));

		if(IsColliding(Box1, Box2))
			RetValue = true;
	}
	if(!RetValue)
	{
		Box1->SetPosition(OriginalPosition1 + (Velocity1 * Negativedt));
		Box2->SetPosition(OriginalPosition2 + (Velocity2 * Negativedt));

		if(IsColliding(Box1, Box2))
			RetValue = true;
	}

	Box1->SetPosition(OriginalPosition1);
	Box2->SetPosition(OriginalPosition2);

	return RetValue;

}

bool CollisionTest::IsColliding(PhysicsBox *Box, D3DXVECTOR3 Point, float Negativedt, D3DXVECTOR3 Velocity1, D3DXVECTOR3 Velocity2)
{
	return IsColliding(Box, Point);
}

bool CollisionTest::IsColliding(CollisionBox Box1, CollisionBox Box2, float Negativedt, D3DXVECTOR3 Velocity1, D3DXVECTOR3 Velocity2)
{
	return IsColliding(Box1, Box2);
}